
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Aircraft.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Aircraft.h"
#include <stdio.h>
#include <stdlib.h>

#include <common/time/gp_time.h>

#include "uITC/uITC.h"
#include "uITC/uITC_msg.h"
#include "SitlSensor/Mag.h"

#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
//设置初始位置 初始偏航角
void Aircraft_SetStartLoc(AirCraft* pAirCraft, const Location start_loc, const float start_yaw)
{
    pAirCraft->home = start_loc;
    pAirCraft->home_yaw = start_yaw;
    pAirCraft->home_is_set = true;

    console_printf("SITL:set Home：Lat = %f  Lng = %f alt = %fm, 航向角：%f",
           pAirCraft->home.lat * 1e-7, 
           pAirCraft->home.lng * 1e-7, 
           pAirCraft->home.alt * 0.01, 
           pAirCraft->home_yaw);

    pAirCraft->location = start_loc;
    pAirCraft->groundlevel = pAirCraft->home.alt * 0.01f;
    pAirCraft->frame_height = 0.0f;        //飞行器高度
    vec3_zero(&(pAirCraft->mag_bf));

    vec3_zero(&(pAirCraft->state.position));
    vec3_zero(&(pAirCraft->state.velocity_ef));
    vec3_zero(&(pAirCraft->state.velocity_bf));
    vec3_zero(&(pAirCraft->state.gyro));


    //设置四元数初始值
    quat_from_euler(&(pAirCraft->state.quat), 0.0f, 0.0f, Deg2Rad(pAirCraft->home_yaw));
}


//返回离地高度
float AirCraft_GetAltAboveGround(AirCraft* pAirCraft)
{
    return - pAirCraft->state.position.z + pAirCraft->home.alt * 0.01f - pAirCraft->groundlevel
           - pAirCraft->frame_height;
}

//根据pos信息 更新Loc
void AirCraft_UpdateLoc(AirCraft* pAirCraft)
{
    pAirCraft->location = pAirCraft->home;
    location_offset(&(pAirCraft->location), pAirCraft->state.position.x, pAirCraft->state.position.y);
    pAirCraft->location.alt = (int32_t)(pAirCraft->home.alt - pAirCraft->state.position.z * 100.0f);
}

void GetRCurvature(float* Rm, float* Rn, float lat)
{
    const float flat = 0.0033528f;
    float eccen2 = 2.0f * flat - flat * flat;

    float var_mid = 1 - eccen2 * SQ(sinf(lat * DEG_TO_RAD));

    *Rn = RADIUS_OF_EARTH / (sqrtf(var_mid));
    *Rm = *Rn * (1.0f - eccen2) / var_mid;
}

void AirCraft_UpdateLoc_v1(AirCraft* pAirCraft, float dt)
{
    float lat = (pAirCraft->location.lat)* 1e-7f;
    //计算曲率半径
    float Rm, Rn;
    GetRCurvature(&Rm, &Rn, lat);

    float dN, dE;
    dN = pAirCraft->state.velocity_ef.x * dt;
    dE = pAirCraft->state.velocity_ef.y * dt;
    
    //更新经纬高
    float dlat, dlon;
    dlon = dE / ((Rn + pAirCraft->location.alt * 0.01f) * cosf(lat * DEG_TO_RAD));
    dlat = dN / (Rm + pAirCraft->location.alt * 0.01f);
    pAirCraft->location.lat += (int32_t)(dlat * 1e7);
    pAirCraft->location.lng += (int32_t)(dlon * 1e7);
    pAirCraft->location.alt = (int32_t)(pAirCraft->home.alt - pAirCraft->state.position.z * 100.0f);
}


//在home点的基础上 加上位置偏置量
void AirCraft_UpdatePos(void)
{
    
}

void AirCraft_InitState(FlyState* pState)
{
    
}


FlyState* AirCraft_GetState(AirCraft* pAircraft)
{
    return &(pAircraft->state);
}

void AirCraft_UpdateDynamics(FlyState* pState, Vector3f_t* rot_acc, Vector3f_t* body_acc, float dt)
{
    ///刚体动力学模型
    Vector3f_t delta_gyro;
    vec3_mult(&delta_gyro, rot_acc, dt);
    vec3_add(&(pState->gyro), &(pState->gyro), &delta_gyro);

    //角速度限幅
    pState->gyro.x = math_constrain_float(pState->gyro.x, -Deg2Rad(2000.0f), Deg2Rad(2000.0f));
    pState->gyro.y = math_constrain_float(pState->gyro.y, -Deg2Rad(2000.0f), Deg2Rad(2000.0f));
    pState->gyro.z = math_constrain_float(pState->gyro.z, -Deg2Rad(2000.0f), Deg2Rad(2000.0f));

    //更新姿态
    Vector3f_t axis_angle;
    vec3_mult(&axis_angle, &(pState->gyro), dt);
    quat_rotate_fast(&(pState->quat), &axis_angle);//小角度快速更新
    quat_norm(&(pState->quat));
    quat_to_euler(&(pState->quat), &(pState->euler));    //更新欧拉角

    ///刚体运动学模型
    Vector3f_t acc_earth;
    pState->acc_body = *body_acc;

    quat_vec_rotate(&(pState->quat), &acc_earth, &(pState->acc_body));
    
    Vector3f_t delta_ve;
    vec3_mult(&delta_ve, &acc_earth, dt);

    //更新速度
    vec3_add(&(pState->velocity_ef), &(pState->velocity_ef), &delta_ve);

    //更新v_b
    quat_inv_vec_rotate(&(pState->quat), &(pState->velocity_bf), &(pState->velocity_ef));

    //更新位置
    Vector3f_t delta_pos;
    vec3_mult(&delta_pos, &(pState->velocity_ef), dt);
    vec3_add(&(pState->position), &(pState->position), &delta_pos);
}

Location AirCraft_GetLocation(AirCraft* pAircraft)
{
    return pAircraft->location;
}

float AirCraft_GetRefAlt(AirCraft* pAircraft)
{
    return (pAircraft->location.alt) * 0.01f;
}

Quat_t* AirCraft_GetQuat(AirCraft* pAircraft)
{
    return &(pAircraft->state.quat);
}

Quat_t AirCraft_GetQuat_q(AirCraft* pAircraft)
{
    return (pAircraft->state.quat);
}


void AirCraft_Publish(AirCraft* pAircraft)
{

    uitc_vehicle_hil_state hil_state;
    hil_state.time_usec = time_micros64();
    hil_state.alt = pAircraft->location.alt * 10;
    hil_state.lat = pAircraft->location.lat;
    hil_state.lon = pAircraft->location.lng;

    hil_state.roll = pAircraft->state.euler.roll;
    hil_state.pitch = pAircraft->state.euler.pitch;
    hil_state.yaw = pAircraft->state.euler.yaw;

    hil_state.rollspeed = pAircraft->state.gyro.x;
    hil_state.pitchspeed = pAircraft->state.gyro.y;
    hil_state.yawspeed = pAircraft->state.gyro.z;

    hil_state.vx = (int16_t)(pAircraft->state.velocity_ef.x * 100);
    hil_state.vy = (int16_t)(pAircraft->state.velocity_ef.y * 100);
    hil_state.vz = (int16_t)(pAircraft->state.velocity_ef.z * 100);

    hil_state.xacc = (int16_t)(pAircraft->state.acc_body.x * 1000 / GRAVITY_MSS);
    hil_state.yacc = (int16_t)(pAircraft->state.acc_body.y * 1000 / GRAVITY_MSS);
    hil_state.zacc = (int16_t)(pAircraft->state.acc_body.z * 1000 / GRAVITY_MSS);
    itc_publish(ITC_ID(vehicle_hil_state), &hil_state);

    //_cprintf("%f\t%f\t%f\t\r\n", hil_state.roll * 57.3, hil_state.pitch * 57.3, hil_state.yaw * 57.3);
}


//根据位置更新磁场强度

void AirCraft_RunMagField(AirCraft* pAircraft)
{
    float intensity;    //磁强度
    float declination;    //磁偏角
    float inclination;    //磁倾角

    Mag_GetField(pAircraft->location.lat * 1.0e-7f, pAircraft->location.lng * 1.0e-7f, &intensity, &declination, &inclination);
    Vector3f_t mag;
    vec3_set(&mag, 1e3f * intensity, 0.0f, 0.0f);

    Quat_t quat;
    quat_from_euler(&quat, 0.0f, -inclination * DEG_TO_RAD, declination * DEG_TO_RAD);

    quat_invert(&quat);
    Vector3f_t mag_ef;
    quat_inv_vec_rotate(&quat, &mag_ef, &mag);

    //计算高度
    const float frame_height = AirCraft_GetAltAboveGround(pAircraft);
    //float anomaly_scaler = 

    //增加干扰量
    quat_inv_vec_rotate(AirCraft_GetQuat(pAircraft), &(pAircraft->mag_bf), &mag_ef);
}

/*------------------------------------test------------------------------------*/


